#include<Servo.h>
#define PIN_SERV1 5 //舵机号（IO口）手抓
#define PIN_SERV2 6 //舵机号（IO口）前臂
#define PIN_SERV3 9 //舵机号（IO口）后臂
#define PIN_SERV4 10 //舵机号（IO口）  底座
#define input1 3 // 定义uno的pin 3 向 input1 输出
#define input2 4 // 定义uno的pin 4 向 input2 输出----//3、4引脚控制左轮
#define input3 7 // 定义uno的pin 5 向 input3 输出
#define input4 8 // 定义uno的pin 6 向 input4 输出----//7、8引脚控制右轮
Servo myservo_1, myservo_2, myservo_3, myservo_4;


void init_Pins(){
  myservo_1.attach(PIN_SERV1);// 将Servo变量附加到引脚，将7号引脚设置为7号舵机控制端口
  myservo_2.attach(PIN_SERV2);
  myservo_3.attach(PIN_SERV3); 
  myservo_4.attach(PIN_SERV4);
  Serial.println("舵机引脚好了！");
  
}
void resetServo(){
  Serial.println("一键舵机复位！");
  myservo_1.write(120);           //使舵机转动至指定角度
  myservo_2.write(0);  
  myservo_3.write(0);  
  myservo_4.write(180);  
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);//初始化串口，并开启调试信息
  Serial.println();
  pinMode(input1, OUTPUT);//小车串口设置为输出模式
  pinMode(input2, OUTPUT);
  pinMode(input3, OUTPUT);
  pinMode(input4, OUTPUT); 
  init_Pins();//舵机引脚并转动范围
  resetServo();
  delay(2000);//初始化延时

}

void loop() {
  // put your main code here, to run repeatedly:
   if(Serial.available()){
    delay(100);
    
    Serial.println(myservo_1.read()+"\n");
    char mode = Serial.read();
    while(Serial.available()>0){
    if(mode=='1'){
      myservo_4.write(180);
      Serial.println("抓到了！");
    }
    
    if(mode=='2'){
      myservo_4.write(0);
      Serial.println("放走了！");
    }
    }
    }
}
